#ifndef __MNM_H
#define __MNM_H

#pragma once


#include <FixedPoint.h>
#include "FixedVec2.h"
#include "FixedVec3.h"
#include "FixedAABB.h"

#if !defined(_RELEASE) || defined(WIN32) || defined(WIN64)
#define DEBUG_MNM_ENABLED 1
#else
#define DEBUG_MNM_ENABLED 0
#endif

// If you want to debug the data consistency just set this define to 1
#define DEBUG_MNM_DATA_CONSISTENCY_ENABLED 0
 
#if defined(WIN32) || defined(WIN64)
#define MNM_USE_EXPORT_INFORMATION		1
#else
#define MNM_USE_EXPORT_INFORMATION		0
#endif

namespace MNM
{
	typedef fixed_t<int, 16>	real_t;
	typedef FixedVec2<int, 16>	vector2_t;
	typedef FixedVec3<int, 16>	vector3_t;
	typedef FixedAABB<int, 16>	aabb_t;

	typedef uint32	TileID;
	typedef uint32	TriangleID;
	typedef uint32	OffMeshLinkID;

	struct OffMeshLink : public _i_reference_target_t
	{
	public:

		enum LinkType
		{
			eType_Invalid = -1,
			eType_SmartObject = 0,
		};

		virtual ~OffMeshLink() {};

	protected:

		OffMeshLink(LinkType _linkType, EntityId _entityId)
			: m_linkType(_linkType)
			, m_entityId(_entityId)
		{

		}

		//Do not implement
		OffMeshLink();

	public:

		EntityId GetEntityId() const { return m_entityId; }

		template<class LinkDataClass>
		LinkDataClass* CastTo()
		{
			return (m_linkType == LinkDataClass::GetType()) ? static_cast<LinkDataClass*>(this) : NULL;
		}

		template<class LinkDataClass>
		const LinkDataClass* CastTo() const
		{
			return (m_linkType == LinkDataClass::GetType()) ? static_cast<const LinkDataClass*>(this) : NULL;
		}

		virtual OffMeshLink* Clone() const = 0;
		virtual Vec3 GetStartPosition() const = 0;

	protected:

		int m_linkType;
		EntityId m_entityId;
	};

	typedef _smart_ptr<OffMeshLink> OffMeshLinkPtr;

	struct WayTriangleData
	{
		WayTriangleData()
		{

		}

		WayTriangleData(const TriangleID _triangleID, const OffMeshLinkID _offMeshLinkID)
			: triangleID(_triangleID)
			, offMeshLinkID(_offMeshLinkID)
		{

		}

		operator bool() const
		{
			return (triangleID != 0);
		}

		bool operator<(const WayTriangleData& other) const
		{
			return triangleID == other.triangleID ? offMeshLinkID < other.offMeshLinkID : triangleID < other.triangleID;
		}

		bool operator==(const WayTriangleData& other) const
		{
			return ((triangleID == other.triangleID) && (offMeshLinkID == other.offMeshLinkID));
		}

		TriangleID		triangleID;
		OffMeshLinkID	offMeshLinkID;
	};

	inline TriangleID ComputeTriangleID(TileID tileID, uint16 triangleIdx)
	{
		return (tileID << 10) | (triangleIdx & ((1 << 10) - 1));
	}

	inline TileID ComputeTileID(TriangleID triangleID)
	{
		return triangleID >> 10;
	}

	inline uint16 ComputeTriangleIndex(TriangleID triangleID)
	{
		return triangleID & ((1 << 10) - 1);
	}

	inline bool IsTriangleAlreadyInWay(const TriangleID triangleID, const TriangleID* const way, const size_t wayTriCount)
	{
		assert(way);
		return std::find(way,way+wayTriCount,triangleID) != (way+wayTriCount);
	}

	struct AStarContention
	{
		void SetFrameTimeQuota(float frameTimeQuota)
		{
			m_frameTimeQuota.SetSeconds(frameTimeQuota);
		}

		struct ContentionStats
		{
			float frameTimeQuota;
			uint32 queueSize;
			uint32 peakQueueSize;

			uint32 averageSearchSteps;
			uint32 peakSearchSteps;

			float averageSearchTime;
			float peakSearchTime;
		};

		ContentionStats GetContentionStats()
		{
			ContentionStats stats;
			stats.frameTimeQuota= m_frameTimeQuota.GetMilliSeconds();

			stats.queueSize = m_queueSize;
			stats.peakQueueSize = m_peakQueueSize;

			stats.averageSearchSteps = m_totalSearchCount ? m_totalSearchSteps / m_totalSearchCount : 0;
			stats.peakSearchSteps = m_peakSearchSteps;

			stats.averageSearchTime = m_totalSearchCount ? m_totalComputationTime / (float)m_totalSearchCount : 0.0f;
			stats.peakSearchTime = m_peakSearchTime;

			return stats;
		}

		void OnUpdateStart(uint32 queueSize)
		{
			m_queueSize = queueSize;
			m_peakQueueSize = max(m_peakQueueSize, queueSize);
			
			m_consumedFrameTime.SetValue(0);
		}

	protected:
		AStarContention(float frameTimeQuota = 0.001f)
		{
			m_frameTimeQuota.SetSeconds(frameTimeQuota);
			ResetContentionStats();
		}

		inline void StartSearch()
		{
			m_currentSearchSteps = 0;
			m_currentSearchTime.SetValue(0);
		}

		inline void EndSearch()
		{
			m_totalSearchCount++;

			m_totalSearchSteps += m_currentSearchSteps;
			m_peakSearchSteps = max(m_peakSearchSteps, m_currentSearchSteps);

			const float lastSearchTime = m_currentSearchTime.GetMilliSeconds();
			m_totalComputationTime += lastSearchTime;
			m_peakSearchTime = max(m_peakSearchTime, lastSearchTime);
		}

		inline void StartStep()
		{
			m_currentStepStartTime = gEnv->pTimer->GetAsyncTime();
			m_currentSearchSteps++;
		}

		inline void EndStep()
		{
			const CTimeValue stepTime = (gEnv->pTimer->GetAsyncTime() - m_currentStepStartTime);
			m_consumedFrameTime += stepTime;
			m_currentSearchTime += stepTime;
		}

		inline bool FrameQuotaReached() const
		{
			return (m_frameTimeQuota > CTimeValue()) ? (m_consumedFrameTime >= m_frameTimeQuota) : false;
		}


		void ResetContentionStats()
		{
			m_consumedFrameTime.SetValue(0);
			m_currentStepStartTime.SetValue(0);

			m_queueSize =  0;
			m_peakQueueSize = 0;

			m_totalSearchCount = 0;

			m_currentSearchSteps = 0;
			m_totalSearchSteps = 0;
			m_peakSearchSteps = 0;

			m_totalComputationTime = 0.0f;
			m_currentSearchTime.SetValue(0);
			m_peakSearchTime = 0.0f;
		}

	protected:
		CTimeValue m_frameTimeQuota;
		CTimeValue m_consumedFrameTime;
		CTimeValue m_currentStepStartTime;
		CTimeValue m_currentSearchTime;
		
		uint32 m_queueSize;
		uint32 m_peakQueueSize;

		uint32 m_totalSearchCount;
		
		uint32 m_currentSearchSteps;
		uint32 m_totalSearchSteps;
		uint32 m_peakSearchSteps;

		float  m_totalComputationTime;
		float  m_peakSearchTime;
	};

	struct AStarOpenList
		: public AStarContention
	{
		typedef AStarContention ContentionPolicy;

		struct Node
		{
			Node()
				: prevTriangle(0, 0)
			{
			}

			Node(TriangleID _prevTriangleID, OffMeshLinkID	_prevOffMeshLinkID, const vector3_t& _location, real_t _cost = 0,
				bool _open = false)
				: prevTriangle(_prevTriangleID, _prevOffMeshLinkID)
				, location(_location)
				, cost(_cost)
				, open(_open)
			{
			}

			bool operator<(const Node& other) const
			{
				return cost < other.cost;
			}

			WayTriangleData prevTriangle;
			vector3_t location;

			real_t cost;
			real_t estimatedTotalCost;

			bool open : 1;
		};

		struct OpenNodeListElement
		{
			OpenNodeListElement(WayTriangleData _triData, Node *_pNode, real_t _fCost) 
				:triData(_triData)
				,pNode(_pNode)
				,fCost(_fCost) 
			{

			}

			WayTriangleData triData;
			Node *pNode;
			real_t fCost;

			bool operator<(const OpenNodeListElement &rOther ) const
			{
				return fCost < rOther.fCost;
			}
		};

		// to get the size of the map entry, use typedef to provide us with value_type
		typedef std::map<WayTriangleData, Node> NodesSizeHelper;
		typedef stl::STLPoolAllocator<NodesSizeHelper::value_type, stl::PSyncNone> OpenListAllocator;
		typedef std::map<WayTriangleData, Node, std::less<WayTriangleData>, OpenListAllocator> Nodes;

		void SetUpForPathSolving(const uint32 triangleCount, const TriangleID fromTriangleID, const vector3_t& startLocation)
		{
			ContentionPolicy::StartSearch();

			const size_t EstimatedNodeCount = triangleCount + 64;
			const size_t MinOpenListSize = NextPowerOfTwo(EstimatedNodeCount);

			m_openList.clear();
			m_openList.reserve(MinOpenListSize);
			m_nodeLookUp.clear();

			std::pair<AStarOpenList::Nodes::iterator,bool> firstEntryIt = m_nodeLookUp.insert( std::make_pair( WayTriangleData(fromTriangleID,0), AStarOpenList::Node(fromTriangleID, 0, startLocation, 0, true) ));				
			m_openList.push_back( OpenNodeListElement(WayTriangleData(fromTriangleID, 0), &firstEntryIt.first->second, real_t()) );

		}

		void PathSolvingDone()
		{
			ContentionPolicy::EndSearch();
		}

		inline OpenNodeListElement PopBestNode()
		{
			ContentionPolicy::StartStep();

			OpenList::iterator it = std::min_element(m_openList.begin(), m_openList.end());

			//Switch the smallest element with the last one and pop the last element
			OpenNodeListElement element = *it;
			*it = m_openList.back();
			m_openList.pop_back();


			return element;
		}

		inline bool InsertNode(const WayTriangleData& triangle, Node** pNextNode)
		{
			std::pair<Nodes::iterator,bool> itInsert = m_nodeLookUp.insert( std::make_pair(triangle, Node()) );
			(*pNextNode) = &itInsert.first->second;

			assert(pNextNode);

			return itInsert.second;
		}

		inline const Node* FindNode(const WayTriangleData& triangle) const
		{
			Nodes::const_iterator it = m_nodeLookUp.find(triangle);

			return (it != m_nodeLookUp.end()) ? &it->second : NULL;
		}

		inline void AddToOpenList(const WayTriangleData triangle, Node* pNode, real_t cost)
		{
			assert(pNode);
			m_openList.push_back( OpenNodeListElement(triangle, pNode, cost ) );
		}

		inline bool CanDoStep() const
		{
			return (!m_openList.empty() && !ContentionPolicy::FrameQuotaReached());
		}

		inline void StepDone() 
		{
			ContentionPolicy::EndStep();
		}

		inline bool Empty() const
		{
			return m_openList.empty();
		}

		void Reset()
		{
			ResetContentionStats();

			stl::free_container(m_openList);
			stl::free_container(m_nodeLookUp);
		}

		bool TileWasVisited(const TileID tileID) const
		{
			Nodes::const_iterator nodesEnd = m_nodeLookUp.end();

			for (Nodes::const_iterator nodeIt = m_nodeLookUp.begin(); nodeIt != nodesEnd; ++nodeIt)
			{
				if (ComputeTileID(nodeIt->first.triangleID) != tileID)
					continue;

				return true;
			}

			return false;
		}

	private:

		size_t NextPowerOfTwo(size_t n)
		{
			n = n - 1;
			n = n | (n >> 1);
			n = n | (n >> 2);
			n = n | (n >> 4);
			n = n | (n >> 8);
			n = n | (n >> 16);
			n = n + 1;

			return n;
		}

		typedef std::vector<OpenNodeListElement> OpenList;
		OpenList m_openList;
		Nodes    m_nodeLookUp;
	};

	template<typename Ty>
	inline bool maximize(Ty& val, const Ty& x)
	{
		if (val < x)
		{
			val = x;
			return true;
		}

		return false;
	}

	template<typename Ty>
	inline bool minimize(Ty& val, const Ty& x)
	{
		if (val > x)
		{
			val = x;
			return true;
		}

		return false;
	}

	template<typename Ty>
	inline void sort2(Ty& x, Ty& y)
	{
		if (x > y)
			std::swap(x, y);
	}

	template<typename Ty>
	inline void rsort2(Ty& x, Ty& y)
	{
		if (x < y)
			std::swap(x, y);
	}

	template<typename Ty>
	inline Ty clamp(const Ty& x, const Ty& min, const Ty& max)
	{
		if (x > max)
			return max;
		if (x < min)
			return min;
		return x;
	}

	template<typename Ty>
	inline Ty clampunit(const Ty& x)
	{
		if (x > Ty(1))
			return Ty(1);
		if (x < Ty(0))
			return Ty(0);
		return x;
	}


	template<typename Ty>
	inline Ty next_mod3(const Ty& x)
	{
		return (x + 1) % 3;
	}

	template<typename VecType>
	inline real_t ProjectionPointLineSeg(const VecType& p, const VecType& a0, const VecType& a1)
	{
		const VecType a = a1 - a0;
		const real_t lenSq = a.lenSq();

		if (lenSq > 0)
		{
			const VecType ap = p - a0;
			const real_t dot = a.dot(ap);
			return dot / lenSq;
		}

		return 0;
	}

	template<typename VecType>
	inline real_t DistPointLineSegSq(const VecType& p, const VecType& a0, const VecType& a1)
	{
		const VecType a = a1 - a0;
		const VecType ap = p - a0;
		const VecType bp = p - a1;

		const real_t e = ap.dot(a);
		if (e <= 0)
			return ap.dot(ap);

		const real_t f = a.dot(a);
		if (e >= f)
			return  bp.dot(bp);

		return ap.dot(ap) - ((e * e) / f);
	}

	template<typename VecType>
	inline VecType ClosestPtPointTriangle(const VecType& p, const VecType& a, const VecType& b, const VecType& c)
	{
		const VecType ab = b - a;
		const	VecType ac = c - a;
		const VecType ap = p - a;

		const real_t d1 = ab.dot(ap);
		const real_t d2 = ac.dot(ap);

		if ((d1 <= 0) && (d2 <= 0))
			return a;

		const VecType bp = p - b;
		const real_t d3 = ab.dot(bp);
		const real_t d4 = ac.dot(bp);

		if ((d3 >= 0) && (d4 <= d3))
			return b;

		const real_t vc = d1 * d4 - d3 * d2;
		if ((vc <= 0) && (d1 >= 0) && (d3 <= 0))
		{
			const real_t v = d1 / (d1 - d3);
			return a + (ab * v);
		}

		const VecType cp = p - c;
		const real_t d5 = ab.dot(cp);
		const real_t d6 = ac.dot(cp);

		if ((d6 >= 0) && (d5 <= d6))
			return c;

		const real_t vb = d5 * d2 - d1 * d6;
		if ((vb <= 0) && (d2 >= 0) && (d6 <= 0))
		{
			const real_t w = d2 / (d2 - d6);
			return a + (ac * w);
		}

		const real_t va = d3 * d6 - d5 * d4;
		if ((va <= 0) && ((d4 - d3) >= 0) && ((d5 - d6) >= 0))
		{
			const real_t w = (d4 - d3) / ((d4 -d3) + (d5 - d6));
			return b + ((c - b) * w);
		}

		// do not factorize the divisions : Fixed point precision requires it
		const real_t denom = va + vb + vc;
		const real_t v = vb / denom;
		const real_t w = vc / denom;

		return a + ab * v + ac * w;
	}

	inline bool PointInTriangle(const vector2_t& p, const vector2_t& a, const vector2_t& b, const vector2_t& c)
	{
		const bool e0 = (p - a).cross(a - b) >= 0;
		const bool e1 = (p - b).cross(b - c) >= 0;
		const bool e2 = (p - c).cross(c - a) >= 0;

		return (e0 == e1) && (e0 == e2);
	}

	template<typename VecType>
	inline real_t ClosestPtSegmentSegment(const VecType& a0, const VecType& a1, const VecType& b0, const VecType& b1,
		real_t& s, real_t& t, VecType& closesta, VecType& closestb)
	{
		const VecType da = a1 - a0;
		const VecType db = b1 - b0;
		const VecType r = a0 - b0;
		const real_t a = da.lenSq();
		const real_t e = db.lenSq();
		const real_t f = db.dot(r);

		if ((a == 0) && (e == 0))
		{
			s = t = 0;
			closesta = a0;
			closestb = b0;

			return (closesta - closestb).lenSq();
		}

		if (a == 0)
		{
			s = 0;
			t = f / e;
			t = clampunit(t);
		}
		else
		{
			const real_t c = da.dot(r);
			
			if (e == 0)
			{
				t = 0;
				s = clampunit(-c / a);
			}
			else
			{
				const real_t b = da.dot(db);
				const real_t denom = (a * e) - (b * b);

				if (denom != 0)
					s = clampunit(((b * f) - (c * e)) / denom);
				else
					s = 0;

				const real_t tnom = (b * s) + f;

				if (tnom < 0)
				{
					t = 0;
					s = clampunit(-c / a);
				}
				else if (tnom > e)
				{
					t = 1;
					s = clampunit((b - c) / a);
				}
				else
					t = tnom / e;
			}
		}

		closesta = a0 + da * s;
		closestb = b0 + db * t;

		return (closesta - closestb).lenSq();
	}

	template<typename VecType>
	inline vector2_t ClosestPtPointSegment(const VecType& p, const VecType& a0, const VecType& a1, real_t& t)
	{
		const VecType a = a1 - a0;

		t = (p - a0).dot(a);

		if (t <= 0)
		{
			t = 0;
			return a0;
		}
		else
		{
			const real_t denom = a.lenSq();

			if (t >= denom)
			{
				t = 1;
				return a1;
			}
			else
			{
				t = t / denom;
				return a0 + (a * t);
			}
		}
	}

	template<typename VecType>
	inline bool IntersectSegmentSegment(const VecType& a0, const VecType& a1, const VecType& b0, const VecType& b1,
		real_t& s, real_t& t)
	{
		const VecType e = b0 - a0;
		const VecType da = a1 - a0;
		const VecType db = b1 - b0;
		const real_t det = da.x * db.y - da.y * db.x;

		if (det != 0)
		{
			s = (e.x * db.y - e.y * db.x) / det;

			if ((s < 0) || (s > 1))
				return false;

			t = (e.x * da.y - e.y * da.x) / det;
			if ((t < 0) || (t > 1))
				return false;

			return true;
		}

		return false;
	}

#if DEBUG_MNM_DATA_CONSISTENCY_ENABLED
	inline void BreakOnInvalidTriangle(TriangleID triangleID, const uint32 tileCount)
	{
		const TileID checkTileID = ComputeTileID(triangleID);
		if ((checkTileID == 0) || (checkTileID > tileCount))
		{
			DEBUG_BREAK;
		}
	}
#else
	inline void BreakOnInvalidTriangle(TriangleID triangleID, const uint32 tileCount)
	{

	}
#endif
}

namespace MNMUtils
{
	inline const MNM::real_t CalculateMinHorizontalRange(const uint16 radiusVoxelUnits, const float voxelHSize) 
	{
		assert(voxelHSize > 0.0f);

		// The horizontal x-y size for the voxels it's recommended to be the same. 
		return MNM::real_t(max(radiusVoxelUnits * voxelHSize * 2.0f, 1.0f));
	}

	inline const MNM::real_t CalculateMinVerticalRange(const uint16 agentHeightVoxelUnits, const float voxelVSize)
	{
		assert(voxelVSize > 0.0f);

		return MNM::real_t(max(agentHeightVoxelUnits * voxelVSize, 1.0f));
	}
}

#endif	// #ifndef __MNM_H
